//
// impressionist.cpp
//
// The main driver program for the other parts. We have two major components,
// UI and Doc.
// They do have a link to each other as their member such that they can 
// communicate.
//

#include <stdio.h>
#include <cstring>
#include <iostream>
#include <cmath>
#include "Impressionist.h"
#include "impressionistUI.h"
#include <Eigenvalues>

using namespace std;
using namespace Eigen;

#define TEXTURE_SIZE 500

Impressionist::Impressionist(){
	m_pUI=NULL;
	picture=NULL;
	width=-1;
	m_scale = 1.0;
}

int Impressionist::pnpoly(int nvert, double *vertx, double *verty, double testx, double testy)
{
  int i, j, c = 0;
  for (i = 0, j = nvert-1; i < nvert; j = i++) {
    if ( ((verty[i]>testy) != (verty[j]>testy)) &&
	 (testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
       c = !c;
  }
  return c;
}

int Impressionist::loadImage(char *fname) 
{
	unsigned char*	data;
	int w,h;
	// try to open the image to read
	if ( (data=readBMP(fname, w, h))==NULL ) 
	{
		return 0;
	}

	width=w;
	height=h;

	m_scale=1.0;
	if (picture ) delete [] picture;
	picture=data;

	// display it on origView
	m_pUI->m_origView->resizeWindow(width, height);	
	m_pUI->m_origView->refresh();
	return 1;
}

/*int Impressionist::loadAndCalculateLines(char *fname)
{
	int x,y;
	FILE* fp = fopen(fname,"r");
	for (int i = 0; i < 12; ++i) {
		fscanf(fp,"%d%d",&x,&y);
		points[i] = MyVector3d(double(x),double(y));
	}
	// Calculate the parallel lines
	for (int i = 0; i < 6; ++i) {
		lines[i] = points[i*2].crossproduct(points[i*2+1]);
	}
	// Calculate vanishing points
	for (int i = 0; i < 3; ++i) {
		vpoints[i] = lines[i*2].crossproduct(lines[i*2+1]);	
		vpoints[i].normalize();
		printf("v points %lf %lf %lf\n",vpoints[i].x,vpoints[i].y,vpoints[i].w);
	}
	// Calculate vanishing line for X-Y plane
	lxy = vpoints[0].crossproduct(vpoints[1]);
	printf("lxy : %lf %lf %lf\n",lxy.x,lxy,y,lxy.w);
	// Load origin and calculate scaling factor
	for (int i = 0; i < 4; ++i) {
		fscanf(fp,"%d%d",&x,&y);
		scene[i] = MyVector3d(double(x),double(y));
		printf("%lf %lf %lf\n",scene[i].x,scene[i].y,scene[i].w);
	}
	for (int i = 0; i < 3; ++i) {
		alpha[i] = D2(scene[i+1],scene[0]);
		printf("%lf ",alpha[i]);
	}
	fclose(fp);
	return 1;
}*/


void Impressionist::calculateVanishingPoints() {
	Matrix3d M;
	Vector3d line;
	for (int j = 0; j < 3; ++j) {
		M = Matrix3d::Zero();
		printf("for point %d total %d\n",j,lines[j].size());
		for (int i = 0; i < lines[j].size(); ++i) {
			line = lines[j][i].first.cross(lines[j][i].second);
			M += line * line.transpose();
			cout << M << endl;
		}
		EigenSolver<Matrix3d> es(M);
		double smallest_eigen_value = es.eigenvalues()[0].real();
		int smallest_eigen_value_index = 0;
		cout << smallest_eigen_value << endl;
		for (int i = 1; i < 3; ++i) {
			double real = es.eigenvalues()[i].real();
			if (real < smallest_eigen_value && real != 0.0) {
				smallest_eigen_value = real;
				smallest_eigen_value_index = i;
			}
		}
		Vector3cd h_complex = es.eigenvectors().col(smallest_eigen_value_index);
		for (int i = 0; i < 3; ++i) {
			vpoints[j][i] = h_complex[i].real();
		}
		vpoints[j] = vpoints[j] / vpoints[j][2];
		cout << "vanishing pts " << vpoints[j] << endl;
	}
}

void Impressionist::calculateHomography() {
	printf("inside calculate homo\n");
	//FILE* fp = fopen("1_homo.txt","r");
	MatrixXd A(8,9);
	MatrixXd AtA(9,9);
	for (int i = 0; i < 4; ++i) {
		//fscanf(fp,"%lf%lf%lf%lf",&p_x[i],&p_y[i],&pp_x[i],&pp_y[i]);
		printf("point %d %lf %lf %lf %lf\n",i,p_x[i],p_y[i],pp_x[i],pp_y[i]);
	}
	A <<
	pp_x[0],pp_y[0],1.0,0.0,0.0,0.0,-p_x[0]*pp_x[0],-p_x[0]*pp_y[0],-p_x[0],
	0.0,0.0,0.0,pp_x[0],pp_y[0],1.0,-p_y[0]*pp_x[0],-p_y[0]*pp_y[0],-p_y[0],
	pp_x[1],pp_y[1],1.0,0.0,0.0,0.0,-p_x[1]*pp_x[1],-p_x[1]*pp_y[1],-p_x[1],
	0.0,0.0,0.0,pp_x[1],pp_y[1],1.0,-p_y[1]*pp_x[1],-p_y[1]*pp_y[1],-p_y[1],
	pp_x[2],pp_y[2],1.0,0.0,0.0,0.0,-p_x[2]*pp_x[2],-p_x[2]*pp_y[2],-p_x[2],
	0.0,0.0,0.0,pp_x[2],pp_y[2],1.0,-p_y[2]*pp_x[2],-p_y[2]*pp_y[2],-p_y[2],
	pp_x[3],pp_y[3],1.0,0.0,0.0,0.0,-p_x[3]*pp_x[3],-p_x[3]*pp_y[3],-p_x[3],
	0.0,0.0,0.0,pp_x[3],pp_y[3],1.0,-p_y[3]*pp_x[3],-p_y[3]*pp_y[3],-p_y[3];
	//cout << A << endl;
	AtA = A.transpose() * A;
	//cout << AtA << endl;
	EigenSolver<MatrixXd> es(AtA);
	//cout << es.eigenvalues() << endl;
	//cout << es.eigenvectors() << endl;
	double smallest_eigen_value = es.eigenvalues()[0].real();
	int smallest_eigen_value_index = 0;
	cout << smallest_eigen_value << endl;
	for (int i = 1; i < 9; ++i) {
		double real = es.eigenvalues()[i].real();
		if (real < smallest_eigen_value && real != 0.0) {
			smallest_eigen_value = real;
			smallest_eigen_value_index = i;
		}
	}
	VectorXcd h_complex = es.eigenvectors().col(smallest_eigen_value_index);
	VectorXd h(9);
	for (int i = 0; i < 9; ++i) {
		h[i] = h_complex[i].real();
	}
	cout << h << endl;
	ground_homo <<  h[0],h[1],h[2],h[3],h[4],h[5],h[6],h[7],h[8];
	cout << "ground homo\n" << ground_homo << endl;
	int x1,y1,x2,y2,length;
	l_xy = vpoints[0].cross(vpoints[1]);
	Vector3d t,b;
	
	//fscanf(fp,"%d%d%d%d%d",&x1,&y1,&x2,&y2,&length);
	//alpha_z = double(length) / sqrt((double(x1)-x2)*(double(x1)-x2) + (double(y1)-y2)*(double(y1)-y2));
	t << t_x,t_y,1;
	b << b_x,b_y,1;
	printf("rz %lf\n",r_z);
	alpha_z = - (l_xy.dot(l_xy) * b.cross(t).norm()) / (b.dot(l_xy) * vpoints[2].cross(t).norm()) / r_z;
	printf("alpha z %lf\n",alpha_z);

	Vector3d test,test_image;
	for (int i = 0; i < 4; ++i) {
		test_image << pp_x[i], pp_y[i],1;
		cout << "test image: " << test_image << endl;
		test = ground_homo * test_image;
		cout << "test " << test << endl;
		printf("%lf %lf %lf\n",test(0),test(1),test(2));
		test(0) /= test(2);
		test(1) /= test(2);
		printf("%lf %lf\n",test(0),test(1));
		printf("before %lf %lf %lf %lf\n",pp_x[i],pp_y[i],p_x[i],p_y[i]);
		//cout << "after projection" << test << endl;
		
	}
	//fclose(fp);
}

void Impressionist::calculatePatches() {
	printf("inside patches");

	char fname[20];

	MatrixXd A(8,9);
	MatrixXd AtA(9,9);
	Matrix3d H;
	VectorXcd h_complex;
	VectorXd h(9);
	Vector3d oldpos,newpos;
	int patch_row, patch_col;
	int patch_width, patch_height;
	//printf("before initialize texture\n");
	/* construct texture*/
	unsigned char*** texture = new unsigned char** [TEXTURE_SIZE];
	for (int i = 0; i < TEXTURE_SIZE; ++i) {
		texture[i] = new unsigned char* [TEXTURE_SIZE];
		for (int j = 0; j < 3; ++j)
			texture[i][j] = new unsigned char [3];
	}
	unsigned char* patch_texture = NULL;
	total_patch = patch_p_x.size();
	//printf("total patch:  %d\n",total_patch);
	for (int i = 0; i < total_patch; ++i) {
		cout << -1 << endl;
		for (int j = 0; j < TEXTURE_SIZE; ++j)
			for (int k = 0; k < 3; ++k)
				memset(texture[j][k],0x0,3 * sizeof(unsigned char));
		cout << 0 << endl;
		/*for (int j = 0; j < 4; ++j) {
			printf("%lf %lf %lf %lf\n",patch_p_x[i][j],patch_p_y[i][j],patch_pp_x[i][j],patch_pp_y[i][j]);
		}*/
		patch_width = 100;//patch_p_y[i][2] - patch_p_y[i][0] + 1;
		patch_height = 100;//patch_p_x[i][1] - patch_p_x[i][0] + 1;
		A <<
		patch_p_x[i][0],patch_p_y[i][0],1.0,0.0,0.0,0.0,-patch_pp_x[i][0]*patch_p_x[i][0],-patch_pp_x[i][0]*patch_p_y[i][0],-patch_pp_x[i][0],
		0.0,0.0,0.0,patch_p_x[i][0],patch_p_y[i][0],1.0,-patch_pp_y[i][0]*patch_p_x[i][0],-patch_pp_y[i][0]*patch_p_y[i][0],-patch_pp_y[i][0],
		patch_p_x[i][1],patch_p_y[i][1],1.0,0.0,0.0,0.0,-patch_pp_x[i][1]*patch_p_x[i][1],-patch_pp_x[i][1]*patch_p_y[i][1],-patch_pp_x[i][1],
		0.0,0.0,0.0,patch_p_x[i][1],patch_p_y[i][1],1.0,-patch_pp_y[i][1]*patch_p_x[i][1],-patch_pp_y[i][1]*patch_p_y[i][1],-patch_pp_y[i][1],
		patch_p_x[i][2],patch_p_y[i][2],1.0,0.0,0.0,0.0,-patch_pp_x[i][2]*patch_p_x[i][2],-patch_pp_x[i][2]*patch_p_y[i][2],-patch_pp_x[i][2],
		0.0,0.0,0.0,patch_p_x[i][2],patch_p_y[i][2],1.0,-patch_pp_y[i][2]*patch_p_x[i][2],-patch_pp_y[i][2]*patch_p_y[i][2],-patch_pp_y[i][2],
		patch_p_x[i][3],patch_p_y[i][3],1.0,0.0,0.0,0.0,-patch_pp_x[i][3]*patch_p_x[i][3],-patch_pp_x[i][3]*patch_p_y[i][3],-patch_pp_x[i][3],
		0.0,0.0,0.0,patch_p_x[i][3],patch_p_y[i][3],1.0,-patch_pp_y[i][3]*patch_p_x[i][3],-patch_pp_y[i][3]*patch_p_y[i][3],-patch_pp_y[i][3];
		//printf("world\n");
		//cout << A << endl;
		AtA = A.transpose() * A;
		//printf("www\n");
		//cout << AtA << endl;
		EigenSolver<MatrixXd> es(AtA);
		//cout << es.eigenvalues() << endl;
		//cout << es.eigenvectors() << endl;
		//printf("hihi\n");
		double smallest_eigen_value = es.eigenvalues()[0].real();
		//printf("hello\n");
		int smallest_eigen_value_index = 0;
		//cout << "here" << endl;
		//cout << smallest_eigen_value << endl;
		for (int i = 1; i < 9; ++i) {
			//printf("now %d\n",i);
			double real = es.eigenvalues()[i].real();
			if (real < smallest_eigen_value && real != 0.0) {
				smallest_eigen_value = real;
				smallest_eigen_value_index = i;
			}
		}
		h_complex = es.eigenvectors().col(smallest_eigen_value_index);
		for (int i = 0; i < 9; ++i) {
			h[i] = h_complex[i].real();
		}
		//printf("before H\n");
		H << h[0],h[1],h[2],h[3],h[4],h[5],h[6],h[7],h[8];
		//printf("after H\n");
		if (patch_texture != NULL)
			delete patch_texture;
		//printf("kkkkk\n");
		patch_texture = new unsigned char [patch_height * patch_width * 3];
		//printf("kkk2\n");
		int ip_h,ip_w;
		for (int p_h = 0; p_h < patch_height; ++p_h) {
			for (int p_w = 0; p_w < patch_width; ++p_w) {
				oldpos << p_h, p_w,1;
				//cout << oldpos << endl;
				newpos = H * oldpos;
				ip_h = height - int(newpos(0) / newpos(2));
				ip_w = int(newpos(1) / newpos(2));
				//printf("%d %d %d %d %d %d %d %d\n",p_h,p_w,int(newpos(0) / newpos(2)),ip_w,ip_h,ip_w,height,width);
				patch_texture[((patch_height - p_h - 1) * patch_width + p_w) * 3] = picture[(ip_h * width + ip_w) * 3];
				patch_texture[((patch_height - p_h - 1) * patch_width + p_w) * 3 + 1] = picture[(ip_h * width + ip_w) * 3 + 1];
				patch_texture[((patch_height - p_h - 1) * patch_width + p_w) * 3 + 2] = picture[(ip_h * width + ip_w) * 3 + 2];
			}
		}
		//printf("before writing the bmp\n");
		sprintf(fname,"patch%d.bmp",i);
		writeBMP(fname,patch_width,patch_height,patch_texture);
	}
}

void Impressionist::setUI(ImpressionistUI* ui){
	m_pUI	= ui;
}

void Impressionist::calculateRealPosition(Point point, Point ground_point) {
	Vector3d ground_pos;
	Vector3d ground_image_pos;
	Vector3d point_image_pos;
	double height;
	point_image_pos << point.x, point.y, 1;
	ground_image_pos << ground_point.x, ground_point.y,1;
	ground_pos = ground_homo * ground_image_pos;
	ground_pos(0) /= ground_pos(2);
	ground_pos(1) /= ground_pos(2);
	height = - (l_xy.dot(l_xy) * ground_image_pos.cross(point_image_pos).norm()) / (ground_image_pos.dot(l_xy) * vpoints[2].cross(point_image_pos).norm()) / alpha_z;
	ground_pos(2) = height;
	printf("point %d %d ground %d %d projected ground pos %lf %lf height %lf\n",point.x,point.y,ground_point.x,
		ground_point.y,ground_pos(0),ground_pos(1),ground_pos(2));
	Vector3i result;
	result << int(ground_pos(0)),int(ground_pos(1)),int(ground_pos(2));
	real_positions.push_back(result);
}

//----------------------------------------------------------------
// Check whether a point is within the range
//----------------------------------------------------------------
bool Impressionist::in_valid_range(int j, int i) {
	if (j < 0 || j >= height)
		return false;
	if (i < 0 || i >= width)
		return false;
	return true;
}

void Impressionist::refresh() {
	if (picture) {
		m_pUI->m_origView->refresh();
	}
}
